#include "ros/ros.h"
#include "geometry_msgs/Pose.h"
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>

#include <sstream>
#include <cstring>
using namespace std;

ros::Publisher pubPose;
ros::Publisher pubString;
ros::Publisher pubGrip;
ros::Publisher pubGetPose;
ros::Publisher pubGetGrip;

void statusCallback(const std_msgs::String& msg)
{
	ROS_INFO("Recieved string: %s", msg.data.c_str());
}

void poseCallback(const geometry_msgs::Pose& msg)
{
	ROS_INFO("Recieved pose: Pos(%f, %f, %f) Ori(%f, %f, %f, %f)",
			 msg.position.x,
			 msg.position.y,
			 msg.position.z,
			 msg.orientation.x,
			 msg.orientation.y,
			 msg.orientation.z,
			 msg.orientation.w);
}

void gripCallback(const std_msgs::Bool& msg)
{
	if(msg.data)
		ROS_INFO("Gripper is open");
	else
		ROS_INFO("Gripper is closed");
}

float xOffset = 400;
float yOffset = 0;
float zOffset = 450;

void MoveTo(float x, float y, float z)
{
	geometry_msgs::Pose msg;

	msg.position.x = x + xOffset;
	msg.position.y = y + yOffset;
	msg.position.z = z + zOffset;
	msg.orientation.x = 0;
	msg.orientation.y = 1;
	msg.orientation.z = 0;
	msg.orientation.w = 0;

	ROS_INFO("Moving to pose: Pos(%f, %f, %f) Ori(%f, %f, %f, %f)",
			 msg.position.x,
			 msg.position.y,
			 msg.position.z,
			 msg.orientation.x,
			 msg.orientation.y,
			 msg.orientation.z,
			 msg.orientation.w);

	pubPose.publish(msg);
}

void GetGripper()
{
	ROS_INFO("Asking robot for gripper");
	std_msgs::Empty msg;
	pubGetGrip.publish(msg);
}

void GetPose()
{
	ROS_INFO("Asking robot for pose");
	std_msgs::Empty msg;
	pubGetPose.publish(msg);
}

void OpenGripper()
{
	ROS_INFO("Opening gripper");
	std_msgs::Bool msg;
	msg.data = true;
	pubGrip.publish(msg);
}

void CloseGripper()
{
	ROS_INFO("Closing gripper");
	std_msgs::Bool msg;
	msg.data = false;
	pubGrip.publish(msg);
}

void timerCallback(const ros::TimerEvent&)
{
	static int state = 0;

	switch(state)
	{
	case 0: //Move to origin position
		MoveTo(0,0,0);
		GetPose();
		break;

	case 1: //Move a bit to the left
		MoveTo(0, 200, 0);
		GetPose();
		break;

	case 2: //Move forward
		MoveTo(200, 200, 0);
		GetPose();
		break;

	case 3: //Move right
		MoveTo(200, 0, 0);
		GetPose();
		break;

	case 4: //Open gripper
		OpenGripper();
		GetGripper();
		break;

	case 5: //Close gripper
		CloseGripper();
		GetGripper();
		break;

	default: break;
	}

	state++;

	if(state >= 6) state = 0;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "dummy");

	ros::NodeHandle n;

	// publish
	pubPose    = n.advertise<geometry_msgs::Pose>("playback",     1000);
	pubString  = n.advertise<std_msgs::String>   ("playbackStr",  1000);
	pubGrip    = n.advertise<std_msgs::Bool>     ("gripper",      1000);
	pubGetPose = n.advertise<std_msgs::Empty>    ("getRobotPose", 1000);
	pubGetGrip = n.advertise<std_msgs::Empty>    ("getRobotGrip", 1000);

	// subscribe
	ros::Subscriber subStatus = n.subscribe("status",    1000, statusCallback);
	ros::Subscriber subPose   = n.subscribe("robotPose", 1000, poseCallback  );
	ros::Subscriber subGrip   = n.subscribe("robotGrip", 1000, gripCallback  );

	ros::Timer timer = n.createTimer(ros::Duration(1), timerCallback);

	ros::spin();

	return 0;
}

